#include <ros/ros.h>
#include <fstream>
#include <iostream>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <tf/transform_datatypes.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <geometric_controller/geometric_controller.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>


using namespace std;
using namespace Eigen;
using namespace message_filters;

std::ofstream outfile;

void callback(const geometry_msgs::PoseStamped::ConstPtr &msg1, const geometry_msgs::PoseStamped::ConstPtr &msg2)
{
    auto time = ros::Time::now().toNSec()/1000000000.0;
    outfile  << setiosflags(ios::fixed) << setprecision(6) 
        << time << ","
        <<msg1->pose.position.x<<","
        <<msg1->pose.position.y<<","
        <<msg1->pose.position.z<<","
        <<msg2->pose.position.x <<","
        <<msg2->pose.position.y<<","
        <<msg2->pose.position.z<<","
        // <<msg1->body_rate.x<<","                     //角速度分量比较
        // <<msg1->body_rate.y<<","
        // <<msg1->body_rate.z<<","
        // <<msg2->twist.angular.x <<","
        // <<msg2->twist.angular.y <<","
        // <<msg2->twist.angular.z <<","
        << endl;
    cout << "synchronized!! " << setiosflags(ios::fixed) << setprecision(6)  << time << endl;

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "data_save");
    ros::NodeHandle nh("~");

    string saveFilePath = "/home/zzx/omega_feedforward/src/geometric_controller/";

    time_t currentTime = std::time(NULL);
    char chCurrentTime[64];
    std::strftime(chCurrentTime,sizeof(chCurrentTime), "%Y-%m-%d-%H-%M-%S", std::localtime(&currentTime));
    std::string stCurrentTime = chCurrentTime;// 转为string

    outfile.open(saveFilePath + "withoutfeed_pid"+ ".txt",  ios::app);

    if(!outfile.is_open())
        {
            cout << "fail to open!" << endl;
            return -1;
        }
    else
    {cout<<"file is open"<<endl;}
    

//message_filters 用于同步的固定名称  mavros_msgs::AttitudeTarget话题的消息类型，可以用 rostopic info +话题名称查找 
    //角速度分量
    // message_filters::Subscriber<mavros_msgs::AttitudeTarget>calcu_bodyrate(nh,"/mavros/setpoint_raw/attitude", 1);  // omega 计算值
    // message_filters::Subscriber<geometry_msgs::TwistStamped> local_bodyrate(nh,"/mavros/local_position/velocity_local", 1);

    message_filters::Subscriber<geometry_msgs::PoseStamped>calcu_bodyrate(nh,"/mavros/local_position/pose", 1);  // 目前位置
    message_filters::Subscriber<geometry_msgs::PoseStamped> local_bodyrate(nh,"/reference/pose", 1);    //参考位置
//calcu_bodyrate 暂代话题名称，仅在同步的两行中使用
        // 角速度分量
    // typedef sync_policies::ApproximateTime<mavros_msgs::AttitudeTarget, geometry_msgs::TwistStamped> MySyncPolicy;
    // Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), calcu_bodyrate, local_bodyrate);
    // sync.registerCallback(boost::bind(callback, _1, _2));

     typedef sync_policies::ApproximateTime<geometry_msgs::PoseStamped, geometry_msgs::PoseStamped> MySyncPolicy;
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), calcu_bodyrate, local_bodyrate);
    sync.registerCallback(boost::bind(callback, _1, _2));

    ros::spin();
    return 0;
}
